#ifndef __MSG_PUBLISH_H__
#define __MSG_PUBLISH_H__

#include <ros/ros.h>

#include <nav_msgs/OccupancyGrid.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/PoseArray.h>
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/Polygon.h>
#include <nav_msgs/Path.h>

#include <cln_msgs/LogMsg.h>

#include <opencv2/opencv.hpp>

using namespace std;

// 路径发布类

class MsgPublish{
    public:
        MsgPublish();
        virtual ~MsgPublish();
        // p2p膨胀地图发布
        void P2pMapGridDilatePub(cv::Mat& image, double& resolution, geometry_msgs::Pose& origin);
        // 全覆盖膨胀地图发布
        void AllCoverMapGridDilatePub(cv::Mat& image, double& resolution, geometry_msgs::Pose& origin);
        // p2p原始路径发布
        void P2pPathOriginPub(std::vector<std::pair<double, double>>& paths);
        // p2p优化路径发布
        void P2pPathOptimizedPub(std::vector<std::pair<double, double>>& paths);
        // 全覆盖路径发布
        void AllCoverPathPub(std::vector<std::vector<std::pair<float, float>>>& paths);
        // P2P标准日志输出
        std::string P2PNormalLogInfo(geometry_msgs::Pose& start,
                                    geometry_msgs::Pose& goal,
                                    double& second, string& info,
                                    int point_size, bool& state);
        // Full Cover Path标准日志输出
        std::string FCPPNormalLogInfo(geometry_msgs::Polygon polygon,
                                    double& second, string& info,
                                    int point_size, bool& state);
        // 任务日志保存
        void SaveLogInfo(string log_level, string msg, int log_code);
        /*日志发布
        *    @log_level 日志等级 DEBUG、INFO、WARN、ERROR、FATAL
        */
        void LogInfoPub(string log_level, vector<string> msgs, int log_code);
    private:
        //节点句柄
        ros::NodeHandle __nh;
        //发布p2p膨胀栅格地图
        ros::Publisher __p2p_Map_Grid_Pub;
        //发布全覆盖膨胀栅格地图
        ros::Publisher __all_Map_Grid_Pub;
        //发布p2p原始路径和优化后的路径
        ros::Publisher __p2p_Path_origin_pub, __p2p_Path_optimized_pub;
        //全局路径发布
        ros::Publisher __all_Cover_Path_Pub;
        //发布日志
        ros::Publisher __log_Info_Pub;
    private:
        // 调试模式 
        bool __debug_Mode;
};
#endif